#ifndef VELPOSE_SIM_CORE_H
#define VELPOSE_SIM_CORE_H

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PointStamped.h"
#include "visualization_msgs/MarkerArray.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf/tf.h"

class UwbSim{
    public:
        UwbSim();
        ~UwbSim();
        void run();
    
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
    
    private:
        tf2_ros::Buffer buffer_;
        tf2_ros::TransformListener listener_;
    
    private:
        std::string input_turtlesim_pose_topic_;
        std::string output_uwb_marker_topic_;
        std::string output_uwb_marker_frame_id_;
    
    private:
        double factor;
    
    private:
        std::shared_ptr<turtlesim::Pose> uwb_pose_;
    
    private:
        ros::Subscriber turtlesim_pose_sub_;
        ros::Subscriber origin_turtlesim_twist_sub_;
        ros::Publisher uwb_marker_pub_;
        ros::Publisher uwb_text_marker_pub_;
        ros::Publisher slow_turtlesim_twist_pub_;
        ros::Publisher global_goal_pose_pub_;
    
    private:
        void process(const ros::TimerEvent& e);
        void turtlesim_pose_callback(const turtlesim::Pose::ConstPtr& msg);
        void origin_turtlesim_twist_callback(const geometry_msgs::Twist::ConstPtr& msg);
};
#endif